#include "EMatrix3x3.h"
#include <assert.h>

/**
 * Returns data at specified row and column. Note that this is a column-major implementation.
 * The first 4 members in memory describe side vector and further ones describe up, forward and translation.
 * @ returns Reference (!) to required matrix element.
 */
template <typename TYPE>
inline TYPE& Matrix3x3<TYPE>::at(uint row, uint column)
{
	assert(row < 3 && column < 3);
	return data2D[column][row];
}

/**
 * Returns data at specified row and column. Note that this is a column-major implementation.
 * The first 4 members in memory describe side vector and further ones describe up, forward and translation.
 * @ returns Value of required matrix element.
 */
template <typename TYPE>
inline TYPE Matrix3x3<TYPE>::get(uint row, uint column) const
{
	assert(row < 3 && column < 3);
	return data2D[column][row];
}

/**
 * Returns data at specified row and column. Note that this is a column-major implementation.
 * The first 4 members in memory describe side vector and further ones describe up, forward and translation.
 * @ returns Value of required matrix element.
 */
template <typename TYPE>
void Matrix3x3<TYPE>::set(uint row, uint column, TYPE value)
{
	assert(row < 3 && column < 3);
	data2D[column][row] = value;
}

template <typename TYPE>
Matrix3x3<TYPE>::Matrix3x3()
{
	// NOP (for performance purposes).
}

template <typename TYPE>
Matrix3x3<TYPE>::Matrix3x3(const Matrix3x3& other)
{
	for (uint r = 0; r < 3; ++r)
	{
		for (uint c = 0; c < 3; ++c)
			this->at(r, c) = other.get(r, c);
	}
}

template <typename TYPE>
Matrix3x3<TYPE>::Matrix3x3(const Vector3<TYPE>& translation) ///< create matrix from translation vector
{
	this->at(0, 0) 	= 1;
	this->at(1, 0) 	= 0;
	this->at(2, 0) 	= 0;

	this->at(0, 1) 	= 0;
	this->at(1, 1) 	= 1;
	this->at(2, 1) 	= 0;

	this->at(0, 2) 	= 0;
	this->at(1, 2) 	= 0;
	this->at(2, 2) 	= 1;

	this->at(0, 3) 	= translation.x;
	this->at(1, 3) 	= translation.y;
	this->at(2, 3) 	= translation.z;
}

template <typename TYPE>
Matrix3x3<TYPE>& Matrix3x3<TYPE>::setIdentity()
{
	this->at(0, 0) 	= 1;
	this->at(1, 0) 	= 0;
	this->at(2, 0) 	= 0;

	this->at(0, 1) 	= 0;
	this->at(1, 1) 	= 1;
	this->at(2, 1) 	= 0;

	this->at(0, 2) 	= 0;
	this->at(1, 2) 	= 0;
	this->at(2, 2) 	= 1;

	this->at(0, 3) 	= 0;
	this->at(1, 3) 	= 0;
	this->at(2, 3) 	= 0;

	return *this;
}

template <typename TYPE>
Matrix3x3<TYPE> Matrix3x3<TYPE>::getIdentity()
{
	Matrix3x3<TYPE> ret;
	return ret.setIdentity();
}

template <typename TYPE>
Matrix3x3<TYPE>& Matrix3x3<TYPE>::transpose()
{
	// TODO: Optimize
	Matrix3x3<TYPE> copy = *this;
	for (int r = 0; r < 3; ++r)
	{
		for (int c = 0; c < 3; ++c)
			this->at(r, c) = copy.at(c, r);
	}
	return *this;
}

template <typename TYPE>
Matrix3x3<TYPE> Matrix3x3<TYPE>::getTransposed() const
{
	Matrix3x3<TYPE> ret;

	for (int r = 0; r < 3; ++r)
	{
		for (int c = 0; c < 3; ++c)
			ret.at(r, c) = this->get(c, r);
	}

	return ret;
}

/**
 * Returns transposed inversed matrix (first transposed, then inversed).
 */
template <typename TYPE>
Matrix3x3<TYPE> Matrix3x3<TYPE>::getInversedTransposed() const
{
	// http://mathworld.wolfram.com/MatrixInverse.html
	// http://stackoverflow.com/questions/983999/simple-3x3-matrix-inverse-code-c

	TYPE determinant =
			+get(0, 0) * (get(1, 1) * get(2, 2) - get(2, 1) * get(1, 2))
			-get(0, 1) * (get(1, 0) * get(2, 2) - get(1, 2) * get(2, 0))
			+get(0, 2) * (get(1, 0) * get(2, 1) - get(1, 1) * get(2, 0));

	assert(determinant != 0);

	TYPE inverseDeterminant = 1.0 / determinant;

	Matrix3x3 ret;

	ret.at(0, 0) =  (get(1, 1) * get(2, 2) - get(2, 1) * get(1, 2)) * inverseDeterminant;
	ret.at(1, 0) = -(get(0, 1) * get(2, 2) - get(0, 2) * get(2, 1)) * inverseDeterminant;
	ret.at(2, 0) =  (get(0, 1) * get(1, 2) - get(0, 2) * get(1, 1)) * inverseDeterminant;

	ret.at(0, 1) = -(get(1, 0) * get(2, 2) - get(1, 2) * get(2, 0)) * inverseDeterminant;
	ret.at(1, 1) =  (get(0, 0) * get(2, 2) - get(0, 2) * get(2, 0)) * inverseDeterminant;
	ret.at(2, 1) = -(get(0, 0) * get(1, 2) - get(1, 0) * get(0, 2)) * inverseDeterminant;

	ret.at(0, 2) =  (get(1, 0) * get(2, 1) - get(2, 0) * get(1, 1)) * inverseDeterminant;
	ret.at(1, 2) = -(get(0, 0) * get(2, 1) - get(2, 0) * get(0, 1)) * inverseDeterminant;
	ret.at(2, 2) =  (get(0, 0) * get(1, 1) - get(1, 0) * get(0, 1)) * inverseDeterminant;

	return *this;
}


// TODO: Check order.
//    static Matrix4x4 createRotationX(TYPE angle)
//    {
//        Matrix4x4 ret;
//
//        ret.at(0, 0) = 1;
//        ret.at(0, 1) = 0;
//        ret.at(0, 2) = 0;
//        ret.at(0, 3) = 0;
//
//        ret.at(1, 0) = 0;
//        ret.at(1, 1) = cos(DEG2RAD(angle));
//        ret.at(1, 2) = -sin(DEG2RAD(angle));
//        ret.at(1, 3) = 0;
//
//        ret.at(2, 0) = 0;
//        ret.at(2, 1) = sin(DEG2RAD(angle));
//        ret.at(2, 2) = cos(DEG2RAD(angle));
//        ret.at(2, 3) = 0;
//
//        ret.at(3, 0) = 0;
//        ret.at(3, 1) = 0;
//        ret.at(3, 2) = 0;
//        ret.at(3, 3) = 1;
//
//        return ret;
//    }
//
//    static Matrix4x4 createRotationY(TYPE angle)
//    {
//        Matrix4x4 ret;
//
//        ret.at(0, 0) = cos(DEG2RAD(angle));
//        ret.at(0, 1) = 0;
//        ret.at(0, 2) = sin(DEG2RAD(angle));
//        ret.at(0, 3) = 0;
//
//        ret.at(1, 0) = 0;
//        ret.at(1, 1) = 1;
//        ret.at(1, 2) = 0;
//        ret.at(1, 3) = 0;
//
//        ret.at(2, 0) = -sin(DEG2RAD(angle));
//        ret.at(2, 1) = 0;
//        ret.at(2, 2) = cos(DEG2RAD(angle));
//        ret.at(2, 3) = 0;
//
//        ret.at(3, 0) = 0;
//        ret.at(3, 1) = 0;
//        ret.at(3, 2) = 0;
//        ret.at(3, 3) = 1;
//
//        return ret;
//    }
//
//    static Matrix4x4 createRotationZ(TYPE angle)
//    {
//        Matrix4x4 ret;
//
//        ret.at(0, 0) = cos(DEG2RAD(angle));
//        ret.at(0, 1) = -sin(DEG2RAD(angle));
//        ret.at(0, 2) = 0;
//        ret.at(0, 3) = 0;
//
//        ret.at(1, 0) = sin(DEG2RAD(angle));;
//        ret.at(1, 1) = cos(DEG2RAD(angle));
//        ret.at(1, 2) = 0;
//        ret.at(1, 3) = 0;
//
//        ret.at(2, 0) = 0;
//        ret.at(2, 1) = 0;
//        ret.at(2, 2) = 1;
//        ret.at(2, 3) = 0;
//
//        ret.at(3, 0) = 0;
//        ret.at(3, 1) = 0;
//        ret.at(3, 2) = 0;
//        ret.at(3, 3) = 1;
//
//        return ret;
//    }


//    Matrix4x4& rotateX(TYPE angle)
//    {
//        return (*this * Matrix4x4::createRotationX(angle));
//    }
//
//    Matrix4x4& rotateY(TYPE angle)
//    {
//    	return (*this * Matrix4x4::createRotationY(angle));
//    }
//
//    Matrix4x4& rotateZ(TYPE angle)
//    {
//    	return (*this * Matrix4x4::createRotationZ(angle));
//    }

template <typename TYPE>
void Matrix3x3<TYPE>::operator*=(const Matrix3x3<TYPE>& other)
{
	Matrix3x3<TYPE> copy(*this);
	TYPE sum;
	for (uint r = 0; r < 3; ++r)
	{
		for (uint c = 0; c < 3; ++c)
		{
			sum = 0;
			for (uint i = 0; i < 3; ++i)
				sum += copy.at(r, i) * other.get(i, c);
			this->at(r, c) = sum;
		}
	}
}

template <typename TYPE>
Matrix3x3<TYPE> operator*(const Matrix3x3<TYPE>& a, const Matrix3x3<TYPE>& b)
{
	Matrix3x3<TYPE> ret;
    TYPE sum;
    for (uint r = 0; r < 3; ++r)
    {
        for (uint c = 0; c < 3; ++c)
        {
            sum = 0;
            for (uint i = 0; i < 4; ++i)
            	sum += a.get(r, i) * b.get(i, c);
            ret.at(r, c) = sum;
        }
    }
    return ret;
}

INSTANTIATE_MATRIX_TEMPLATE(Matrix3x3)

template Matrix3x3<float> 	operator*(const Matrix3x3<float>&  a, const Matrix3x3<float>&  b);
template Matrix3x3<double> 	operator*(const Matrix3x3<double>& a, const Matrix3x3<double>& b);

